package org.example.util;


import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import org.example.bean.*;

import java.util.*;

/**
 * @Desc:寻路算法工具类
 * @Author: yanghongjun
 * @Date: 2024/5/14
 **/
public class ThetaStarUtil {
    private static final Logger logger = LogManager.getLogger(ThetaStarUtil.class);


    /**
     * 地图寻路
     *
     * @param start   起点
     * @param end     终点
     * @param gameMap 地图数据
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    public static List<JsonGamePos> findPath(JsonGamePos start, JsonGamePos end, GameMap gameMap, List<Long> passIds, boolean checkPass) {
        //近距离小格子寻路
        long now = System.currentTimeMillis();
        passIds.clear();
        List<JsonGamePos> pathGird = new ArrayList<>();
        int startTerritory = gameMap.getGirdByPos(start).getTerritory();
        int endTerritory = gameMap.getGirdByPos(end).getTerritory();
        if (startTerritory == endTerritory) {
            //起点和终点可以直线相连就直接设置为路径
            if (gameMap.canConnectStraight(start, end)) {
                pathGird.add(start);
                pathGird.add(end);
                logger.info("直线相连");
                return pathGird;
            }
            List<JsonGamePos> pathBigGirdLazy = findPathBigGirdLazy(start, end, gameMap);
            if (pathBigGirdLazy != null) {
                pathGird = pathBigGirdLazy;
            }
        } else {
            //上一个关卡id
            long lastPassId = 0;
            //上一个疆域id
            int lastTerritoryId = startTerritory;
            //上一个路径点
            JsonGamePos lastPos = start;
            if (!checkPass) {
                //找到起点到终点要通过的关卡和疆域
                LinkedHashMap<MapPass, Integer> pathBetweenTerritories = gameMap.findPathBetweenTerritories(start, end);
                if (!pathBetweenTerritories.isEmpty()) {
                    for (Map.Entry<MapPass, Integer> mapPassIntegerEntry : pathBetweenTerritories.entrySet()) {
                        MapPass mapPass = mapPassIntegerEntry.getKey();
                        Integer territoryId = mapPassIntegerEntry.getValue();
                        if (lastPassId > 0) {
                            //使用关卡之间缓存的路径
                            List<JsonGamePos> jsonGamePos = mapPass.getPathHashMap().get(lastPassId);
                            List<JsonGamePos> reversed = new ArrayList<>(jsonGamePos);
                            Collections.reverse(reversed);
                            pathGird.addAll(reversed);
                        } else {
                            Cord cord = mapPass.getCordHashMap().get(lastTerritoryId);
                            List<JsonGamePos> pathBigGird = findPathBigGirdLazy(lastPos, cord.toPos(), gameMap);
                            if (pathBigGird == null || pathBigGird.isEmpty()) {
                                pathGird.clear();
                                break;
                            }
                            pathGird.addAll(pathBigGird);
                        }
                        lastPos = mapPass.getCordHashMap().get(territoryId).toPos();
                        pathGird.add(mapPass.getCordHashMap().get(territoryId).toPos());
                        lastTerritoryId = territoryId;
                        lastPassId = mapPass.getId();
                        passIds.add(lastPassId);
                    }
                    //关卡到终点的路径
                    List<JsonGamePos> pathBigGird = findPathBigGirdLazy(lastPos, end, gameMap);
                    if (pathBigGird != null) {
                        pathGird.remove(pathBigGird.get(0));
                        pathGird.addAll(pathBigGird);
                    }
                }
            } else {
                logger.error("跨关卡寻路失败   start：{} end：{}", start, end);
            }
        }
        if (pathGird.isEmpty()) {
            logger.error("寻路失败 start：{} end：{}", start, end);
        } else {
            logger.info("寻路 耗时：{} 节点:{}", (System.currentTimeMillis() - now), pathGird.size());
        }
        return pathGird;
    }


    /**
     * 大格子寻路（使用 Lazy Theta* 算法）
     *
     * @param start   起点坐标（JsonGamePos 格式）
     * @param end     终点坐标（JsonGamePos 格式）
     * @param gameMap 游戏地图数据，提供地图信息和 LOS 检查等方法
     * @return 从起点到终点的路径列表，若无法到达则返回 null
     */
    public static List<JsonGamePos> findPathBigGirdLazy(JsonGamePos start, JsonGamePos end, GameMap gameMap) {
        Grid startGird = gameMap.getGirdByPos(start);
        Grid endGird = gameMap.getGirdByPos(end);
        if (startGird.getTerritory() != endGird.getTerritory()) {
            logger.error("起点和终点疆域不一致 start:{} end:{}", start, end);
            return null;
        }
        // 记录寻路开始时间，用于性能监控
        long now = System.currentTimeMillis();
        // 初始化起始节点和目标节点，由 GameMap 提供转换功能
        Node startNode = null;
        Node goalNode = null;
        //用于记录所有获取的 Node，便于统一释放
        List<Node> allAcquiredNodes = new ArrayList<>();
        try {
            // 从 GameMap 获取对应的 Node 表示
            startNode = gameMap.JsonGamePosToMapGirdIndex(start);
            goalNode = gameMap.JsonGamePosToMapGirdIndex(end);
            allAcquiredNodes.add(startNode);
            allAcquiredNodes.add(goalNode);
            // 设置最大开放列表大小为地图总格子数，防止死循环或无限扩展
            int maxsize = gameMap.getMapWidth() * gameMap.getMapHeight();
            // 使用优先队列维护 OpenSet，按 fScore 排序（gScore + hScore）
            PriorityQueue<Node> openSet = new PriorityQueue<>(Comparator.comparingDouble(Node::getfScore));
            // 使用 Map 快速判断节点是否在 OpenSet 中（避免 PriorityQueue.contains 的 O(n) 性能问题）
            Map<Node, Node> openSetMap = new HashMap<>();
            // ClosedSet 存储已访问过的节点
            Set<Node> closedSet = new HashSet<>();
            // 初始化起点节点
            startNode.setgScore(0); // 到起点的实际代价为 0
            startNode.sethScore(heuristic(startNode, goalNode)); // 启发函数估算到终点的距离
            startNode.setfScore(startNode.getgScore() + startNode.gethScore()); // f = g + h
            // 将起点加入 OpenSet
            openSet.add(startNode);
            openSetMap.put(startNode, startNode);
            startNode.setInOpenSet(true);
            // 开始主循环：从 OpenSet 中取出当前最优节点继续搜索
            while (!openSet.isEmpty()) {
                // 防止异常情况下 OpenSet 无限增长
                if (openSet.size() > maxsize) {
                    logger.error("findPathBigGird openSet exceed");
                    return null;
                }
                // 取出当前 fScore 最小的节点
                Node currentNode = openSet.poll();
                openSetMap.remove(currentNode); // 同步更新 map
                currentNode.setInOpenSet(false); // 标记为不在 OpenSet 中
                // 如果当前节点是目标节点，则找到路径
                if (currentNode.equals(goalNode)) {
                    // 构建路径列表
                    List<Cord> pathList = reconstructBigPath(currentNode);
                    // 优化路径（如去除中间冗余点）
                    List<JsonGamePos> result = refinePathGird(pathList, gameMap, start, end);
                    // 输出耗时日志
                    //logger.info("Lazy Theta* 大格子寻路耗时：{}", (System.currentTimeMillis() - now));
                    return result;
                }
                // 将当前节点加入 ClosedSet，表示已处理
                closedSet.add(currentNode);
                // 获取当前节点的所有相邻节点（通常是 8 方向）
                List<Node> neighbors = gameMap.getGirdNeighbors(currentNode, goalNode);
                // 将邻居节点加入 allAcquiredNodes 列表
                //allAcquiredNodes.addAll(neighbors);
                for (Node neighbor : neighbors) {
                    // 如果邻居已在 ClosedSet 或 OpenSet 中，跳过
                    if (closedSet.contains(neighbor) || openSetMap.containsKey(neighbor)) {
                        continue;
                    }
                    // Lazy Theta* 核心：尝试通过 LOS 直接连接父节点
                    Node parent = currentNode.getParent();
                    int tentativeG;
                    Node newPathParent = currentNode;
                    // 如果当前节点有父节点，并且父节点与邻居之间可视（LOS），则直接走父节点路径
                    if (parent != null && gameMap.canConnectStraightGrid(parent.getX(), parent.getY(), neighbor.getX(), neighbor.getY())) {
                        tentativeG = parent.getgScore() + distBetween(parent, neighbor);
                        newPathParent = parent;
                    } else {
                        tentativeG = currentNode.getgScore() + distBetween(currentNode, neighbor);
                    }
                    // 如果找到了更短的路径，更新邻居节点的信息
                    if (!neighbor.isInOpenSet() || tentativeG < neighbor.getgScore()) {
                        neighbor.setParent(newPathParent);
                        neighbor.setgScore(tentativeG);
                        neighbor.sethScore(heuristic(neighbor, goalNode));
                        neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                        // 加入 OpenSet 和 Map
                        openSet.add(neighbor);
                        openSetMap.put(neighbor, neighbor);
                        neighbor.setInOpenSet(true);
                    }
                }
            }
            // 找不到路径，记录错误日志
            logger.error("找不到从 {} 到 {} 的路径", start, end);
            return null;
        } finally {
            // 无论是否成功寻路，都必须将所有节点释放回池中
            releaseAllNodes(allAcquiredNodes, gameMap.getNodePool());
        }
    }

    /**
     * 大格子寻路的路径上进行细化，阻挡为GameGridMap的blocks
     *
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    private static List<JsonGamePos> refinePathGird(List<Cord> list, GameMap gameMap, JsonGamePos start, JsonGamePos end) {
        if (list.isEmpty()) {
            return Collections.emptyList();
        }
        List<JsonGamePos> path = new ArrayList<>();
        path.add(start);
        // 中间点：将大格子路径转为对应中心点（但不包括首尾格子）
        int size = list.size();
        for (int i = 1; i < size - 1; i++) {
            Cord cord = list.get(i);
            JsonGamePos centerPos = gameMap.mapToGameCoords(cord.getX(), cord.getY());
            path.add(centerPos);
        }
        path.add(end);
        return gameMap.optimizeJsonGamePosPathGrid(path);
    }


    /**
     * 根据给定的当前节点，重建并返回从起点到该节点的路径。
     *
     * @param currentNode 当前节点
     * @return 从起点到当前节点的路径
     */
    private static List<Cord> reconstructBigPath(Node currentNode) {
        List<Cord> path = new ArrayList<>();
        while (currentNode != null) {
            int x = currentNode.getX() * 10 + 5;
            int y = currentNode.getY() * 10 + 5;
            path.add(new Cord(x, y)); // 深拷贝
            currentNode = currentNode.getParent();
        }
        Collections.reverse(path);
        return path;
    }

    /**
     * 释放节点回对象池
     *
     * @param nodes
     * @param pool
     */
    private static void releaseAllNodes(List<Node> nodes, ThreadLocalNodePool pool) {
        if (nodes == null || pool == null) {
            return;
        }
        for (Node node : nodes) {
            pool.release(node);
        }
        nodes.clear(); // 清空列表防止重复释放或内存占用
    }

    /**
     * 计算两个节点之间的启发式距离  并非最优路径，但是寻路会更快
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 启发式距离
     */
    private static int heuristic(Node nodeA, Node nodeB) {
        int dx = Math.abs(nodeA.getX() - nodeB.getX());
        int dy = Math.abs(nodeA.getY() - nodeB.getY());
        return 10 * (dx + dy) + (14 - 20) * Math.min(dx, dy);
    }

    /**
     * 计算两个节点之间的启发式距离。曼哈顿距离距离
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 启发式距离
     */
//    private static int heuristic(JsonGamePos nodeA, JsonGamePos nodeB) {
//        return (int) (Math.abs(nodeA.getX() - nodeB.getX()) + Math.abs(nodeA.getY() - nodeB.getY())) * LINEAR_COST;
//    }

    /**
     * 计算两个相邻节点的移动代价。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 两点之间的距离
     */
    private static int distBetween(Node nodeA, Node nodeB) {
        int dx = Math.abs(nodeA.getX() - nodeB.getX());
        int dy = Math.abs(nodeA.getY() - nodeB.getY());

        if (dx == 1 && dy == 0 || dy == 1 && dx == 0) {
            return 10; // 正向移动
        } else if (dx == 1 && dy == 1) {
            return 14; // 对角线移动
        } else {
            return (int) (Math.hypot(dx, dy) * 10); // LOS 或跳跃连接
        }
    }
}
